// robot_arm_kinematics.h
#ifndef ROBOT_ARM_KINEMATICS_H
#define ROBOT_ARM_KINEMATICS_H

#include <stdint.h>
#include <stdbool.h>
#include "Usart.h"

// 机械臂参数结构体（与您原有代码兼容）
typedef struct {
    uint8_t pwm;
    uint8_t left_limit;
    uint8_t middle;
    uint8_t right_limit;
} SERVO_PARAM;

// 标定参数
typedef struct {
    float scale_x;          // 像素到厘米的比例（X轴）
    float scale_z;          // 像素到厘米的比例（Z轴）
    float delta_x;          // 摄像头光心到机械臂基座的水平偏移
    float delta_z;          // 摄像头光心到机械臂基座的垂直偏移
    float L1;               // 大臂长度（cm）
    float L2;               // 小臂长度（cm）
} ArmCalibration;

typedef struct {
    int16_t m1;
    int16_t m2;
    int16_t m3;
    int16_t m4;
}M_Target;

// 初始化标定参数
void Arm_InitCalibration(ArmCalibration *calib);

// 计算所有舵机PWM值
void Calculate_Target_PWM(
    ArmCalibration *calib, 
    int cx_pixel, int cz_pixel, 
    uint8_t *pwm_left,   // 左舵机（上下）
    uint8_t *pwm_right,  // 右舵机（前后）
    uint8_t *pwm_up      // 前舵机（夹爪，可选）
);

typedef enum { 
    GRIPPER_IDLE,
    GRIPPER_PENDING 
} GripperState;
bool ShouldTriggerGripper(void);
M_Target CheckAndMoveToCenter(SpitballCoordinates coords);

#endif
